#include <stdlib.h>
#include <stdio.h>
#include <assert.h>
#include <float.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <sys/time.h>
#include "common.h"

double size;

//  tuned constants
#define density 0.0005
#define mass    0.01
#define cutoff  0.01
#define min_r   (cutoff/100)
#define dt      0.0005
#define TRUE 1
#define FALSE 0

//  timer
double read_timer( )
{
	static bool initialized = false;
	static struct timeval start;
	struct timeval end;
	if( !initialized )
	{
		gettimeofday( &start, NULL );
		initialized = true;
	}
	gettimeofday( &end, NULL );
	return (end.tv_sec - start.tv_sec) + 1.0e-6 * (end.tv_usec - start.tv_usec);
}

//  keep density constant
void set_size( int n )
{
	size = sqrt( density * n );
}

//  Initialize the particle positions and velocities
void init_particles( int n, particle_t *p )
{
	srand48( time( NULL ) );

	int sx = (int)ceil(sqrt((double)n));
	int sy = (n+sx-1)/sx;

	int *shuffle = (int*)malloc( n * sizeof(int) );
	for( int i = 0; i < n; i++ )
		shuffle[i] = i;

	for( int i = 0; i < n; i++ ) 
	{
		//
		//  make sure particles are not spatially sorted
		//
		int j = lrand48()%(n-i);
		int k = shuffle[j];
		shuffle[j] = shuffle[n-i-1];

		//
		//  distribute particles evenly to ensure proper spacing
		//
		p[i].x = size*(1.+(k%sx))/(1+sx);
		p[i].y = size*(1.+(k/sx))/(1+sy);

		//
		//  assign random velocities within a bound
		//
		p[i].vx = drand48()*2-1;
		p[i].vy = drand48()*2-1;
	}
	free( shuffle );
}

//  interact two particles
void apply_force( particle_t &particle, particle_t &neighbor )
{
	double dx = neighbor.x - particle.x;
	double dy = neighbor.y - particle.y;
	double r2 = dx * dx + dy * dy;
	if( r2 > cutoff*cutoff )
		return;
	r2 = max( r2, min_r*min_r );
	double r = sqrt( r2 );

	//
	//  very simple short-range repulsive force
	//
	double coef = ( 1 - cutoff / r ) / r2 / mass;
	particle.ax += coef * dx;
	particle.ay += coef * dy;
}


// apply_force_double() works just like apple_force(), except
// that is applies the force in both directions
void apply_force_double( particle_t &particle, particle_t &neighbor )
{
	double dx = neighbor.x - particle.x;
	double dy = neighbor.y - particle.y;
	double r2 = dx * dx + dy * dy;
	if( r2 > cutoff*cutoff )
		return;
	r2 = max( r2, min_r*min_r );
	double r = sqrt( r2 );

	//
	//  very simple short-range repulsive force
	//
	double coef = ( 1 - cutoff / r ) / r2 / mass;
	particle.ax += coef * dx;
	particle.ay += coef * dy;
	neighbor.ax -= coef * dx;
	neighbor.ay -= coef * dy;
}

// apply_force_pb is just like apple_force, except
// that it applies the force between a particle and a box
void apply_force_pb(particle_t &particle, box &box)
{
	double dx = box.cmx - particle.x;	// Use the boxes center of mass as coordinates
	double dy = box.cmy - particle.y;
	double r2 = dx * dx + dy * dy;
	if( r2 > cutoff*cutoff )
		return;
	r2 = max( r2, min_r*min_r );
	double r = sqrt( r2 );

	//
	//  very simple short-range repulsive force
	//
	double coef = ( 1 - cutoff / r ) / r2 / box.tmass;	// User the boxes mass
	particle.ax += coef * dx;
	particle.ay += coef * dy;
}

//  integrate the ODE
void move( particle_t &p )
{
	//
	//  slightly simplified Velocity Verlet integration
	//  conserves energy better than explicit Euler method
	//
	p.vx += p.ax * dt;
	p.vy += p.ay * dt;
	p.x  += p.vx * dt;
	p.y  += p.vy * dt;

	//
	//  bounce from walls
	//
	while( p.x < 0 || p.x > size )
	{
		p.x  = p.x < 0 ? -p.x : 2*size-p.x;
		p.vx = -p.vx;
	}
	while( p.y < 0 || p.y > size )
	{
		p.y  = p.y < 0 ? -p.y : 2*size-p.y;
		p.vy = -p.vy;
	}
}

//  I/O routines
void save( FILE *f, int n, particle_t *p )
{
	static bool first = true;
	if( first )
	{
		fprintf( f, "%d %g\n", n, size );
		first = false;
	}
	for( int i = 0; i < n; i++ )
		fprintf( f, "%g %g\n", p[i].x, p[i].y );
}

// mp_save() is just like save() except it takes particles 
// as a vector instead of an array 
void mp_save( FILE *f, int n, vector< particle_t > &p )
{
	static bool first = true;
	if( first )
	{
		fprintf( f, "%d %g\n", n, size );
		first = false;
	}
	for( int i = 0; i < n; i++ )
		fprintf( f, "%g %g\n", p[i].x, p[i].y );
}

//  command line option processing
int find_option( int argc, char **argv, const char *option )
{
	for( int i = 1; i < argc; i++ )
		if( strcmp( argv[i], option ) == 0 )
			return i;
	return -1;
}

int read_int( int argc, char **argv, const char *option, int default_value )
{
	int iplace = find_option( argc, argv, option );
	if( iplace >= 0 && iplace < argc-1 )
		return atoi( argv[iplace+1] );
	return default_value;
}

char *read_string( int argc, char **argv, const char *option, char *default_value )
{
	int iplace = find_option( argc, argv, option );
	if( iplace >= 0 && iplace < argc-1 )
		return argv[iplace+1];
	return default_value;
}


//-------------------------------------------


// Initializes the world root box
void init_world(box *world)
{
	world->init(0, size, 0, size);
	world->index.first = 0;
	world->index.second = 0;
	world->level = 0;
}

// Goes through a 2 dimensional array and creates an array with boxes
// in each position of the array
void initializations(box ***boxes, int target_level)
{
	int i;
	int ii;
	int size;
	for(i = 0; i < target_level + 1; i++)
	{	
		size = 1 << i;

		boxes[i] = new box*[size];
		for(ii = 0; ii < size; ii++)
		{
			boxes[i][ii] = new box[size];
		}
	}
}



// This function goes through all of the boxes between box (x, y) and box(x2, y2)
// each box gets split (gets children initialised, assigned, etc...)
// after all of the boxes have been created and initialized
// neighbors are assigned, this is done after, and not during the same process
// because we must ensure all of the boxes actually are assigned properly on all of the 
// layers
void build_tree(box ***boxes, particle_t *particles, int level, int depth_level, int x, int y, int x2, int y2)	
{
	int j;
	int k;

	int tmplevel = level;
	int tmpx = x;
	int tmpy = y;
	int tmpx2 = x2;
	int tmpy2 = y2;

	int cx = x;
	int cy = y;
	while(level < depth_level)
	{
		split(boxes[level][cx][cy], particles, boxes, cx, cy);
		if (cy < y2)
		{
			cy += 1;
		}
		else if (cx < x2)
		{
			cy = y;
			cx ++;
		} else
		{
			x *= 2;
			y *= 2;
			cx = x;
			cy = y;
			level ++;
			x2 *= 2;
			y2 *= 2;
			x2 ++;
			y2 ++;
		}
	}

	level = tmplevel;
	cx = tmpx;
	cy = tmpy;
	x2 = tmpx2;
	y2 = tmpy2;
	while(level < depth_level + 1)
	{
		// Goes to the parent box
		box *tmp = &boxes[level - 1][boxes[level][cx][cy].parent.first][boxes[level][cx][cy].parent.second];
		// Goes through all of the parents neighbors neighbors
		for(j = 0; j < tmp->neighbor.size(); j++)
		{	
			// Gets a neighbor
			box *neighbor = &boxes[tmp->level][tmp->neighbor[j].first][tmp->neighbor[j].second];

			// Checks the neighbors children
			for(k = 0; k < 4; k++)
			{
				// Gets a child that then is at the same layer as one self is
				box *child = &boxes[level][neighbor->children[k].first][neighbor->children[k].second];
				if(check_neighbor(boxes[level][cx][cy], *child))
				{
					// if the box is neighbor, set it as a neighbor
					boxes[level][cx][cy].set_neighbor(child->index.first, child->index.second);
				}
				else
				{
					// If it is not a neighbor, set it as a an interaction box
					// these boxes you just treat as big particles instead
					boxes[level][cx][cy].set_interact(child->index.first, child->index.second);
				}
			}
		}

		//--------------------
		if (cy < y2)
		{
			cy += 1;
		}
		else if (cx < x2)
		{
			cy = y;
			cx ++;
		} else
		{
			x *= 2;
			y *= 2;
			cx = x;
			cy = y;
			level ++;
			x2 *= 2;
			y2 *= 2;
			x2 ++;
			y2 ++;
		}
	}
}

// This function will take a box and add all of its children
// that are at the bottom level to the box_list
void find_bottom_box(box &b, box ***boxes, vector< vector< pair < int, int > > >& box_list, int depth_level, int x, int y)
{
	// Children are at the level bellow so we must
	// multiply the coordinates by two to make it correct
	x = x * 2;
	y = y * 2;

	// Assigns the children
	box *children[4];
	children[0] = &(boxes[b.level + 1][x][y]);
	children[1] = &(boxes[b.level + 1][x][y + 1]);
	children[2] = &(boxes[b.level + 1][x + 1][y]);
	children[3] = &(boxes[b.level + 1][x + 1][y + 1]);

	pair<int, int> p(b.index.first, b.index.second);
	box_list[b.level].push_back(p);

	if(children[0]->level == depth_level)
	{
		// If we are at the bottom, add to bottom list
		pair<int, int> pair1(children[0]->index.first, children[0]->index.second);
		pair<int, int> pair2(children[1]->index.first, children[1]->index.second);
		pair<int, int> pair3(children[2]->index.first, children[2]->index.second);
		pair<int, int> pair4(children[3]->index.first, children[3]->index.second);

		box_list[b.level + 1].push_back(pair1);
		box_list[b.level + 1].push_back(pair2);
		box_list[b.level + 1].push_back(pair3);
		box_list[b.level + 1].push_back(pair4);
		return ;
	}
	else
	{
		// if not at the bottom, keep going down
		find_bottom_box(*children[0], boxes, box_list, depth_level, children[0]->index.first, children[0]->index.second);
		find_bottom_box(*children[1], boxes, box_list, depth_level, children[1]->index.first, children[1]->index.second);
		find_bottom_box(*children[2], boxes, box_list, depth_level, children[2]->index.first, children[2]->index.second);
		find_bottom_box(*children[3], boxes, box_list, depth_level, children[3]->index.first, children[3]->index.second);
	}
}

// Checks if two boxes are next to eachother/neighbors by
// checking the difference in index between them
int check_neighbor(box &box1, box &box2)
{
	if(abs(box1.index.first - box2.index.first) <= 1 && abs(box1.index.second - box2.index.second) <= 1)
		return TRUE;

	return FALSE;
}

// This function goes through the boxes between box (x, y) and box (x2, y2)
// and sorts the particles in those boxes to their children
void assign_elements(box ***boxes, particle_t *particles, int level, int depth_level, int x, int y, int x2, int y2)	
{
	int cx = x;
	int cy = y;
	while(level < depth_level)
	{	
		sort_elements(boxes[level][cx][cy], particles, boxes, cx, cy);
		if (cy < y2)
		{
			cy += 1;
		}
		else if (cx < x2)
		{
			cy = y;
			cx ++;
		} else
		{
			x *= 2;
			y *= 2;
			cx = x;
			cy = y;
			level ++;
			x2 *= 2;
			y2 *= 2;
			x2 ++;
			y2 ++;
		}
	}
}

// This function takes a box and sorts its particles to its children
void sort_elements(box &b, particle_t *particles, box ***boxes, int x, int y)
{
	// Children are at the level bellow so we must
	// multiply the coordinates by two to make it correct
	x = x * 2;
	y = y * 2;
	// Assigns the children
	box *children[4];
	children[0] = &(boxes[b.level + 1][x][y]);
	children[1] = &(boxes[b.level + 1][x][y + 1]);
	children[2] = &(boxes[b.level + 1][x + 1][y]);
	children[3] = &(boxes[b.level + 1][x + 1][y + 1]);

	// Clears the particle lists 
	children[0]->element.clear();
	children[1]->element.clear();
	children[2]->element.clear();
	children[3]->element.clear();
	// Clears the box masses
	children[0]->tmass = 0;
	children[1]->tmass = 0;
	children[2]->tmass = 0;
	children[3]->tmass = 0;

	// Assign particles to each boxes;
	for(int i = 0; i < b.element.size(); i ++)
	{
		particle_t *p = &particles[b.element[i]];
		if(p->x < children[0]->x2)
		{
			if(p->y < children[0]->y2){
				children[0]->add_new(b.element[i]);
			}
			else{
				children[2]->add_new(b.element[i]);
			}
		} else
		{
			if(p->y < children[1]->y2)
				children[1]->add_new(b.element[i]);
			else
				children[3]->add_new(b.element[i]);
		}
	}
}

// This function gets a box
// and creates and initializes its children 
// and sets references the neighbor boxes
void split(box &b, particle_t *particles, box ***boxes, int x, int y)
{
	int i;

	// Children are at the level bellow so we must
	// multiply the coordinates by two to make it correct
	x = x * 2;
	y = y * 2;
	// Assigns the children
	box *children[4];
	children[0] = &(boxes[b.level + 1][x][y]);
	children[1] = &(boxes[b.level + 1][x][y + 1]);
	children[2] = &(boxes[b.level + 1][x + 1][y]);
	children[3] = &(boxes[b.level + 1][x + 1][y + 1]);

	// Create 4 boxes with boundaries
	children[0]->init(b.x1, (b.x2 - b.x1) / 2 + b.x1, b.y1, (b.y2 - b.y1) / 2 + b.y1);
	children[1]->init((b.x2 - b.x1) / 2 + b.x1, b.x2, b.y1, (b.y2 - b.y1) / 2 + b.y1);
	children[2]->init(b.x1, (b.x2 - b.x1) / 2 + b.x1, (b.y2 - b.y1) / 2 + b.y1, b.y2);
	children[3]->init((b.x2 - b.x1) / 2 + b.x1, b.x2, (b.y2 - b.y1) / 2 + b.y1, b.y2);
	// Sets the children boxes their coordinates
	children[0]->index.first = x;		children[0]->index.second = y;
	children[1]->index.first = x;		children[1]->index.second = y + 1;
	children[2]->index.first = x + 1;	children[2]->index.second = y;
	children[3]->index.first = x + 1;	children[3]->index.second = y + 1;

	// Assign particles to each boxes;
	for(i = 0; i < b.element.size(); i ++)
	{
		particle_t *p = &particles[b.element[i]];
		if(p->x < children[0]->x2)
		{
			if(p->y < children[0]->y2){
				children[0]->add_new(b.element[i]);
			}
			else{
				children[2]->add_new(b.element[i]);
			}
		} else
		{
			if(p->y < children[1]->y2)
				children[1]->add_new(b.element[i]);
			else
				children[3]->add_new(b.element[i]);
		}
	}

	// Sets this box as each childs parent
	// sets their level, center of mass and mass
	// and adds them as children to this box
	for(i = 0; i < 4; i++)
	{
		children[i]->set_parent(b.index.first, b.index.second);
		children[i]->level = b.level + 1;

		// After all of the particles have been added to the box
		// We need to calculate the center of mass for each box
		children[i]->calculate_cm(particles);

		boxes[b.level][b.index.first][b.index.second].set_children(i, children[i]->index.first, children[i]->index.second);
	}

	// Sets the neighbor boxes to all of the children
	int m;
	for(int n = 0; n < 4; n ++)
		for(m = 0; m < 4; m++)
			if(n != m)
				boxes[b.level + 1][children[n]->index.first][children[n]->index.second].set_neighbor(children[m]->index.first, children[m]->index.second);
}

// This is Fast Multipole Method
void fmm(box ***boxes, particle_t *particles, int depth_level, vector < vector < pair <int, int> > > &box_list)
{	
	int i;
	int j;
	int k;

	// Level 0: do nothing, we only hae 1 big box
	// Level 1: do nothing, we still have no well separated boxes
	for(int level = 2; level <= depth_level; level++)
	{
		for(int bi = 0; bi < box_list[level].size(); bi++)
		{
			// Gets the box we are at
			box b = (boxes[level][box_list[level][bi].first][box_list[level][bi].second]);

			// if between level 1 and the bottom only apply force to
			// the big distant box particles as a whole
			if(level > 1 && level != depth_level)
			{
				// Goes through every element in this box
				// Apple force between each particle and a distant box
				for(i = 0; i < b.element.size(); i++)
					for(j = 0; j < b.interaction_list.size(); j ++)
						apply_force_pb(particles[b.element[i]], boxes[level][b.interaction_list[j].first][b.interaction_list[j].second]);
			}


			// At this level multipole expansion has been calculated on some particles and on their well separeted boxes
			// On level 3 we need to split the level 2 NEIGHBOURS and performe multipole
			// expansion between them. The level 2 well separeted boxes have alreade been accounted for.
			if(level == depth_level)
			{
				// Goes through every element in this box
				for(i = 0; i < b.element.size(); i++)
				{
					// Performes multipole expansion on every box that is well seperated
					for(j = 0; j < b.interaction_list.size(); j ++)
					{
						//box tmp = boxes[b.interaction_list[j]];
						apply_force_pb(particles[b.element[i]], boxes[level][b.interaction_list[j].first][b.interaction_list[j].second]);	// p1, tmp
					}

					// Applies force on every particle with every particle in the near neighbours list
					for(j = 0; j < b.neighbor.size(); j ++)
					{
						boxes[level][b.neighbor[j].first][b.neighbor[j].second].remove_neighbor(b.index.first, b.index.second);
						for(k = 0; k < boxes[level][b.neighbor[j].first][b.neighbor[j].second].element.size(); k++)
						{
							// Apply the force both ways because it halvs the amount of force calculations
							apply_force_double(particles[b.element[i]], particles[boxes[level][b.neighbor[j].first][b.neighbor[j].second].element[k]]); //
						}
					}
				}

				// Then you only have your own box to calculate
				// Go through all of your particles and calculate 
				j = 0;
				for(i = 0; i < b.element.size(); i++)
				{
					j = i + 1;
					while(j < b.element.size())
					{	
						apply_force_double(particles[b.element[i]], particles[b.element[j]]);
						j++;
					}
				}
			}

		}
	}

	/*
	while(level < depth_level + 1)
	{
		box b = boxes[level][cx][cy];

		if(level > 1 && level != depth_level)
		{
			// Goes through every element in this box
			for(i = 0; i < b.element.size(); i++)
			{
				for(j = 0; j < b.interaction_list.size(); j ++)
				{
					apply_force_pb(particles[b.element[i]], boxes[level][b.interaction_list[j].first][b.interaction_list[j].second]);
				}
			}
		}

		// At this level multipole expansion has been calculated on some particles and on their well separeted boxes
		// On level 3 we need to split the level 2 NEIGHBOURS and performe multipole
		// expansion between them. The level 2 well separeted boxes have alreade been accounted for.
		// 

		if(level == depth_level)
		{
			// Goes through every element in this box
			for(i = 0; i < b.element.size(); i++)
			{
				// Performes multipole expansion on every box that is well seperated
				for(j = 0; j < b.interaction_list.size(); j ++)
				{
					//box tmp = boxes[b.interaction_list[j]];
					apply_force_pb(particles[b.element[i]], boxes[level][b.interaction_list[j].first][b.interaction_list[j].second]);	// p1, tmp
				}

				// Applies force on every particle with every particle in the near neighbours list
				for(j = 0; j < b.neighbor.size(); j ++)
				{
					boxes[level][b.neighbor[j].first][b.neighbor[j].second].remove_neighbor(cx, cy);
					for(k = 0; k < boxes[level][b.neighbor[j].first][b.neighbor[j].second].element.size(); k++)
					{
						apply_force_double(particles[b.element[i]], particles[boxes[level][b.neighbor[j].first][b.neighbor[j].second].element[k]]); //
					}
				}
			}

			//-----------

			j = 0;

			for(i = 0; i < b.element.size(); i++)
			{
				j = i + 1;
				while(j < b.element.size())
				{	
					apply_force_double(particles[b.element[i]], particles[b.element[j]]);
					j++;
				}
			}
		}

	if (cy < y2)
	{
	cy ++;
	}
	else if (cx < x2)
	{
	cy = y;
	cx ++;
	} else
	{
	x *= 2;
	y *= 2;
	cx = x;
	cy = y;
	level ++;
	x2 *= 2;
	y2 *= 2;
	x2 ++;
	y2 ++;
	}
	}
	*/
}

// This is Fast Multipole Method
// This version uses an older version which traverses through the boxes
// instead of going through the box_list like the other fmm versions do.
// This is because we concluded that changing this version to the same one 
// as the other would result in too many changes which would be requerd in
// openmp.cpp and the gain would be too little
void omp_fmm(box ***boxes, particle_t *particles, int level, int x, int y, int x2, int y2)
{	
	int cx = x;
	int cy = y;
	int i;
	int j;
	int k;
	
	while(level < 4)
	{
		box b = boxes[level][cx][cy];

		// Level 0: do nothing, we only hae 1 big box
		// Level 1: do nothing, we still have no well separated boxes
		if(level == 2)
		{
			// Goes through every element in this box
			// Apple force between each particle and a distant box
			for(i = 0; i < b.element.size(); i++)
			{
				for(j = 0; j < b.interaction_list.size(); j ++)
				{
					apply_force_pb(particles[b.element[i]], boxes[level][b.interaction_list[j].first][b.interaction_list[j].second]);
				}
			}
		}

		// At this level multipole expansion has been calculated on some particles and on their well separeted boxes
		// On level 3 we need to split the level 2 NEIGHBOURS and performe multipole
		// expansion between them. The level 2 well separeted boxes have alreade been accounted for.
		if(level == 3)
		{
			// Goes through every element in this box
			for(i = 0; i < b.element.size(); i++)
			{
				// Performes multipole expansion on every box that is well seperated
				for(j = 0; j < b.interaction_list.size(); j ++)
				{
					//box tmp = boxes[b.interaction_list[j]];
					apply_force_pb(particles[b.element[i]], boxes[level][b.interaction_list[j].first][b.interaction_list[j].second]);	// p1, tmp
				}

				// Applies force on every particle with every particle in the near neighbours list
				for(j = 0; j < b.neighbor.size(); j ++)
				{
					for(k = 0; k < boxes[level][b.neighbor[j].first][b.neighbor[j].second].element.size(); k++)
					{
						// Apply the force both ways because it halvs the amount of force calculations
						apply_force(particles[b.element[i]], particles[boxes[level][b.neighbor[j].first][b.neighbor[j].second].element[k]]); //
					}
				}
			}

			// Then you only have your own box to calculate
			// Go through all of your particles and calculate 

			j = 0;

			for(i = 0; i < b.element.size(); i++)
			{
				j = i + 1;
				while(j < b.element.size())
				{	
					apply_force_double(particles[b.element[i]], particles[b.element[j]]);
					j++;
				}
			}
		}

		if (cy < y2)
		{
			cy ++;
		}
		else if (cx < x2)
		{
			cy = y;
			cx ++;
		} else
		{
			x *= 2;
			y *= 2;
			cx = x;
			cy = y;
			level ++;
			x2 *= 2;
			y2 *= 2;
			x2 ++;
			y2 ++;
		}
	}
}


//-------------------------------------------


//	Functions for MP version only
void mp_apply_force_pb(particle_t &particle, mp_box &box)
{
	double dx = box.cmx - particle.x;	// Use the boxes center of mass as coordinates
	double dy = box.cmy - particle.y;
	double r2 = dx * dx + dy * dy;
	if( r2 > cutoff*cutoff )
		return;
	r2 = max( r2, min_r*min_r );
	double r = sqrt( r2 );

	//  very simple short-range repulsive force
	double coef = ( 1 - cutoff / r ) / r2 / box.tmass;	// User the boxes mass
	particle.ax += coef * dx;
	particle.ay += coef * dy;
}

void mp_init_world(mp_box *world)
{
	world->init(0, size, 0, size);
	world->index.first = 0;
	world->index.second = 0;
	world->level = 0;
}

// Goes through a 2 dimensional array and creates an array with boxes
// in each position of the array
void mp_initializations(mp_box ***boxes, int target_level)
{
	int i;
	int ii;
	int size;
	for(i = 0; i < target_level + 1; i++)
	{	
		size = 1 << i;

		boxes[i] = new mp_box*[size];
		for(ii = 0; ii < size; ii++)
		{
			boxes[i][ii] = new mp_box[size];
		}
	}
}




// This function goes through all of the boxes between box (x, y) and box(x2, y2)
// each box gets split (gets children initialised, assigned, etc...)
// after all of the boxes have been created and initialized
// neighbors are assigned, this is done after, and not during the same process
// because we must ensure all of the boxes actually are assigned properly on all of the 
// layers
void mp_build_tree(mp_box ***boxes, int level, int depth_level, int x, int y, int x2, int y2)
{
	int j;
	int k;

	int tmplevel = level;
	int tmpx = x;
	int tmpy = y;
	int tmpx2 = x2;
	int tmpy2 = y2;

	int cx = x;
	int cy = y;
	while(level < depth_level)
	{
		mp_split(boxes[level][cx][cy], boxes, cx, cy);
		if (cy < y2)
		{
			cy += 1;
		}
		else if (cx < x2)
		{
			cy = y;
			cx ++;
		} else
		{
			x *= 2;
			y *= 2;
			cx = x;
			cy = y;
			level ++;
			x2 *= 2;
			y2 *= 2;
			x2 ++;
			y2 ++;
		}
	}

	level = tmplevel;
	cx = tmpx;
	cy = tmpy;
	x2 = tmpx2;
	y2 = tmpy2;
	while(level < depth_level + 1)
	{
		// Goes to the parent box
		mp_box *tmp = &boxes[level - 1][boxes[level][cx][cy].parent.first][boxes[level][cx][cy].parent.second];
		// Goes through all of the parents neighbors neighbors
		for(j = 0; j < tmp->neighbor.size(); j++)
		{	
			// Gets a neighbor
			mp_box *neighbor = &boxes[tmp->level][tmp->neighbor[j].first][tmp->neighbor[j].second];

			// Checks the neighbors children
			for(k = 0; k < 4; k++)
			{
				// Gets a child that then is at the same layer as one self is
				mp_box *child = &boxes[level][neighbor->children[k].first][neighbor->children[k].second];
				if(mp_check_neighbor(boxes[level][cx][cy], *child))
				{
					// if the box is neighbor, set it as a neighbor
					boxes[level][cx][cy].set_neighbor(child->index.first, child->index.second);
				}
				else
				{
					// If it is not a neighbor, set it as a an interaction box
					// these boxes you just treat as big particles instead
					boxes[level][cx][cy].set_interact(child->index.first, child->index.second);
				}
			}
		}

		//--------------------
		if (cy < y2)
		{
			cy += 1;
		}
		else if (cx < x2)
		{
			cy = y;
			cx ++;
		} else
		{
			x *= 2;
			y *= 2;
			cx = x;
			cy = y;
			level ++;
			x2 *= 2;
			y2 *= 2;
			x2 ++;
			y2 ++;
		}
	}
}

// This function will take a box and add all of its children
// that are at the bottom level to the box_list
void mp_find_bottom_box(mp_box &b, mp_box ***boxes, vector< vector< pair < int, int > > >& box_list, int depth_level, int x, int y)
{
	// Children are at the level bellow so we must
	// multiply the coordinates by two to make it correct
	x = x * 2;
	y = y * 2;

	// Assigns the children
	mp_box *children[4];
	children[0] = &(boxes[b.level + 1][x][y]);
	children[1] = &(boxes[b.level + 1][x][y + 1]);
	children[2] = &(boxes[b.level + 1][x + 1][y]);
	children[3] = &(boxes[b.level + 1][x + 1][y + 1]);

	pair<int, int> p(b.index.first, b.index.second);
	box_list[b.level].push_back(p);

	if(children[0]->level == depth_level)
	{
		// If we are at the bottom, add to bottom list
		pair<int, int> pair1(children[0]->index.first, children[0]->index.second);
		pair<int, int> pair2(children[1]->index.first, children[1]->index.second);
		pair<int, int> pair3(children[2]->index.first, children[2]->index.second);
		pair<int, int> pair4(children[3]->index.first, children[3]->index.second);

		box_list[b.level + 1].push_back(pair1);
		box_list[b.level + 1].push_back(pair2);
		box_list[b.level + 1].push_back(pair3);
		box_list[b.level + 1].push_back(pair4);
		return ;
	}
	else
	{
		// if not at the bottom, keep going down
		mp_find_bottom_box(*children[0], boxes, box_list, depth_level, children[0]->index.first, children[0]->index.second);
		mp_find_bottom_box(*children[1], boxes, box_list, depth_level, children[1]->index.first, children[1]->index.second);
		mp_find_bottom_box(*children[2], boxes, box_list, depth_level, children[2]->index.first, children[2]->index.second);
		mp_find_bottom_box(*children[3], boxes, box_list, depth_level, children[3]->index.first, children[3]->index.second);
	}
}

// Checks if two boxes are next to eachother/neighbors by
// checking the difference in index between them
int mp_check_neighbor(mp_box &box1, mp_box &box2)
{
	if(abs(box1.index.first - box2.index.first) <= 1 && abs(box1.index.second - box2.index.second) <= 1)
		return TRUE;

	return FALSE;
}

// This function goes through the boxes between box (x, y) and box (x2, y2)
// and sorts the particles in those boxes to their children
void mp_assign_elements(mp_box ***boxes, int level, int depth_level, int x, int y, int x2, int y2)
{
	int cx = x;
	int cy = y;
	while(level < depth_level)
	{	
		mp_sort_elements(boxes[level][cx][cy], boxes, cx, cy);
		if (cy < y2)
		{
			cy += 1;
		}
		else if (cx < x2)
		{
			cy = y;
			cx ++;
		} else
		{
			x *= 2;
			y *= 2;
			cx = x;
			cy = y;
			level ++;
			x2 *= 2;
			y2 *= 2;
			x2 ++;
			y2 ++;
		}
	}
}

// This function takes a box and sorts its particles to its children
void mp_sort_elements(mp_box &b, mp_box ***boxes, int x, int y)
{
	// Children are at the level bellow so we must
	// multiply the coordinates by two to make it correct
	x = x * 2;
	y = y * 2;
	// Assigns the children
	mp_box *children[4];
	children[0] = &(boxes[b.level + 1][x][y]);
	children[1] = &(boxes[b.level + 1][x][y + 1]);
	children[2] = &(boxes[b.level + 1][x + 1][y]);
	children[3] = &(boxes[b.level + 1][x + 1][y + 1]);

	// Clears the particle lists 
	children[0]->element.clear();
	children[1]->element.clear();
	children[2]->element.clear();
	children[3]->element.clear();
	// Clears the box masses
	children[0]->tmass = 0;
	children[1]->tmass = 0;
	children[2]->tmass = 0;
	children[3]->tmass = 0;

	// Assign particles to each boxes;
	for(int i = 0; i < b.element.size(); i ++)
	{
		particle_t *p = &b.element[i];
		if(p->x < children[0]->x2)
		{
			if(p->y < children[0]->y2){
				children[0]->add_new(b.element[i]);
			}
			else{
				children[2]->add_new(b.element[i]);
			}
		} else
		{
			if(p->y < children[1]->y2)
				children[1]->add_new(b.element[i]);
			else
				children[3]->add_new(b.element[i]);
		}
	}
}

// This function gets a box
// and creates and initializes its children 
// and sets references the neighbor boxes
void mp_split(mp_box &b, mp_box ***boxes, int x, int y)
{
	int i;

	// Children are at the level bellow so we must
	// multiply the coordinates by two to make it correct
	x = x * 2;
	y = y * 2;
	// Assigns the children
	mp_box *children[4];
	children[0] = &(boxes[b.level + 1][x][y]);
	children[1] = &(boxes[b.level + 1][x][y + 1]);
	children[2] = &(boxes[b.level + 1][x + 1][y]);
	children[3] = &(boxes[b.level + 1][x + 1][y + 1]);

	// Create 4 boxes with boundaries
	children[0]->init(b.x1, (b.x2 - b.x1) / 2 + b.x1, b.y1, (b.y2 - b.y1) / 2 + b.y1);
	children[1]->init((b.x2 - b.x1) / 2 + b.x1, b.x2, b.y1, (b.y2 - b.y1) / 2 + b.y1);
	children[2]->init(b.x1, (b.x2 - b.x1) / 2 + b.x1, (b.y2 - b.y1) / 2 + b.y1, b.y2);
	children[3]->init((b.x2 - b.x1) / 2 + b.x1, b.x2, (b.y2 - b.y1) / 2 + b.y1, b.y2);
	// Sets the children boxes their coordinates
	children[0]->index.first = x;		children[0]->index.second = y;
	children[1]->index.first = x;		children[1]->index.second = y + 1;
	children[2]->index.first = x + 1;	children[2]->index.second = y;
	children[3]->index.first = x + 1;	children[3]->index.second = y + 1;

	// Assign particles to each boxes;
	for(i = 0; i < b.element.size(); i ++)
	{
		particle_t *p = &b.element[i];
		if(p->x < children[0]->x2)
		{
			if(p->y < children[0]->y2){
				children[0]->add_new(b.element[i]);
			}
			else{
				children[2]->add_new(b.element[i]);
			}
		} else
		{
			if(p->y < children[1]->y2)
				children[1]->add_new(b.element[i]);
			else
				children[3]->add_new(b.element[i]);
		}
	}

	// Sets this box as each childs parent
	// sets their level, center of mass and mass
	// and adds them as children to this box
	for(i = 0; i < 4; i++)
	{
		children[i]->set_parent(b.index.first, b.index.second);
		children[i]->level = b.level + 1;

		// After all of the particles have been added to the box
		// We need to calculate the center of mass for each box
		children[i]->calculate_cm();

		boxes[b.level][b.index.first][b.index.second].set_children(i, children[i]->index.first, children[i]->index.second);
	}

	// Sets the neighbor boxes to all of the children
	int m;
	for(int n = 0; n < 4; n ++)
		for(m = 0; m < 4; m++)
			if(n != m)
				boxes[b.level + 1][children[n]->index.first][children[n]->index.second].set_neighbor(children[m]->index.first, children[m]->index.second);
}

// This is Fast Multipole Method
void mp_fmm(mp_box ***boxes,  int depth_level, vector < vector < pair <int, int> > > &box_list)
{
	int i;
	int j;
	int k;

	// Level 0: do nothing, we only hae 1 big box
	// Level 1: do nothing, we still have no well separated boxes
	//2
	for(int level = 2; level <= depth_level; level++)
	{
		for(int bi = 0; bi < box_list[level].size(); bi++)
		{
			// Gets the box we are at
			mp_box *b = &(boxes[level][box_list[level][bi].first][box_list[level][bi].second]);

			// if between level 1 and the bottom only apply force to
			// the big distant box particles as a whole
			if(level > 1 && level != depth_level)
			{
				// Goes through every element in this box
				// Apple force between each particle and a distant box
				for(i = 0; i < b->element.size(); i++)
					for(j = 0; j < b->interaction_list.size(); j ++)
						mp_apply_force_pb(b->element[i], boxes[level][b->interaction_list[j].first][b->interaction_list[j].second]);

				mp_sort_elements(*b, boxes, box_list[level][bi].first, box_list[level][bi].second);
			}

			if(level == depth_level)
			{
				for(i = 0; i < b->element.size(); i++)
				{
					// Performes multipole expansion on every box that is well seperated
					for(j = 0; j < b->interaction_list.size(); j ++)
					{
						mp_apply_force_pb(b->element[i], boxes[level][b->interaction_list[j].first][b->interaction_list[j].second]);	// p1, tmp
					}

					// Applies force on every particle with every particle in the near neighbours list
					for(j = 0; j < b->neighbor.size(); j ++)
					{
						boxes[level][b->neighbor[j].first][b->neighbor[j].second].remove_neighbor(b->index.first, b->index.second);
						for(k = 0; k < boxes[level][b->neighbor[j].first][b->neighbor[j].second].element.size(); k++)
						{
							// Apply the force both ways because it halvs the amount of force calculations
							apply_force_double(b->element[i], boxes[level][b->neighbor[j].first][b->neighbor[j].second].element[k]); 
						}
					}
				}

				// Then you only have your own box to calculate
				// Go through all of your particles and calculate 
				int j = 0;
				for(i = 0; i < b->element.size(); i++)
				{
					j = i + 1;
					while(j < b->element.size())
					{	
						apply_force_double(b->element[i], b->element[j]);
						j++;
					}
				}
			}
		}
	}
}
